#include "rs485.h"
#include <unistd.h>

shared_ptr<RS485_Handler> RS485_Handler::instance = nullptr;

RS485_Handler::RS485_Handler()
{

}

void RS485_Handler::init(string dev, int band_rate, bool thread_safe, bool debug_mode)
{
    if(m_init_flag)
        return ;
    m_thread_safe = thread_safe;

    ctx = modbus_new_rtu(dev.c_str(), band_rate, 'N', 8 ,1);
    modbus_set_debug(ctx, debug_mode ? TRUE : FALSE);
    
    modbus_set_error_recovery(ctx,
                            (modbus_error_recovery_mode)
                            (MODBUS_ERROR_RECOVERY_LINK | 
                            MODBUS_ERROR_RECOVERY_PROTOCOL));

    modbus_set_response_timeout(ctx, 0u, 4000u * 1000u);

    while (modbus_connect(ctx) == -1)
    {
        cout << "modbus 485 Connection failed" << endl;
        cout << "modbus errno" << modbus_strerror(errno) << endl;
        sleep(1);
    }

    inner_thread = new thread(&RS485_Handler::_inner_exec, this);
    inner_thread->detach();
    m_init_flag = true;
}

int RS485_Handler::ReadInputRegister(int slave_id, vector<uint16_t> &data, int addr, int nb)
{
    UpdateSlavesID(slave_id);
    return WR_Reg_Inner(slave_id, data, addr, nb, 1);
}

int RS485_Handler::ReadHoldingRegister(int slave_id, vector<uint16_t> &data, int addr, int nb)
{
    UpdateSlavesID(slave_id);
    return WR_Reg_Inner(slave_id, data, addr, nb, 2);
}

int RS485_Handler::WriteHoldingRegister(int slave_id, vector<uint16_t> &data, int addr, int nb)
{
    UpdateSlavesID(slave_id);
    return WR_Reg_Inner(slave_id, data, addr, nb, 3);
}

int RS485_Handler::WR_Reg_Inner(int slave_id, vector<uint16_t> &data, int addr, int nb, int mode)
{
    if(m_thread_safe)
        rd_lock.lock();

    if(data.size()<nb)
    {
        data.resize(nb);
    }
    modbus_set_slave(ctx, slave_id);
    // while (modbus_connect(ctx) == -1)
    // {
    //     cout << "modbus 485 Connection failed with\t" << slave_id << endl;
    //     cout << "modbus errno" << modbus_strerror(errno) << endl;
    //     sleep(1);
    // }
    int rs485_res = 0;
    switch (mode)
    {
    case 1:
        rs485_res = modbus_read_input_registers(ctx, addr, nb, data.data());
        break;
    case 2:
        rs485_res = modbus_read_registers(ctx, addr, nb, data.data());
        break;
    case 3:
        rs485_res = modbus_write_registers(ctx, addr, nb, data.data());
        break;
    default:
        cerr << "rs485 RW Inner error" << endl;
        break;
    }
    int rs485_err_cnt = 0;
    while (-1 == rs485_res)
    {
        rs485_err_cnt ++;
        if(rs485_err_cnt>=10) {
            cerr << "modbus can not transmit!" << endl;
            return -1;
        }
        cerr << "modbus transmit errno:\t" << errno << endl;
        if(errno != 110) {
            modbus_close(ctx);
            while (modbus_connect(ctx)==-1)
            {
                rs485_err_cnt += 2;
                if(rs485_err_cnt>=10) {
                    cerr << "modbus can not connect!" << endl;
                    return -1;
                }
                cerr << "reconnect_flag fail with\t" << slave_id << endl;
			    sleep(1);
                
            }
        }
        switch (mode)
        {
        case 1:
            rs485_res = modbus_read_input_registers(ctx, addr, nb, data.data());
            break;
        case 2:
            rs485_res = modbus_read_registers(ctx, addr, nb, data.data());
            break;
        case 3:
            rs485_res = modbus_write_registers(ctx, addr, nb, data.data());
            break;
        default:
            cerr << "rs485 RW Inner error" << endl;
            break;
        }
    }
    // modbus_close(ctx);
    if(m_thread_safe)
        rd_lock.unlock();
    return 0;
}

void RS485_Handler::_inner_exec()
{
    vector<uint16_t> input_buf(1);
    vector<uint16_t> holding_buf(1);
    while(1)
    {
        for(auto it = m_slaves_id.begin();it != m_slaves_id.end();++it)
        {
            int transmit_res = WR_Reg_Inner(*it, input_buf  , 0, 1, 1);
            if(transmit_res != 0) exit(-1);
            //can transmit error
            if(input_buf[0] == 1)
            {
                m_slave_can_error_flag = 1;
            }
            transmit_res = WR_Reg_Inner(*it, holding_buf, 0, 1, 2);
            if(transmit_res != 0) exit(-1);
            if(input_buf[0] == 0)
            {
                m_slave_reset_error_flag = 1;
            }
        }
        sleep(30);
    }
}

RS485_Handler::~RS485_Handler()
{
    // modbus_close(ctx);
    if(inner_thread)
    {
        delete inner_thread;
    }
    modbus_close(ctx);
    modbus_free(ctx);
    ctx = nullptr;
}

void RS485_Handler::UpdateSlavesID(int slave_id)
{
    if(m_thread_safe)
        update_slaves_id_lock.lock();
    auto it = m_slaves_id.find(slave_id);
    if(it == m_slaves_id.end()) 
    {
        //set addr0 to 1.
        vector<uint16_t> buf(1);
        buf[0] = 1;
        WR_Reg_Inner(slave_id, buf, 0, 1, 3);
        m_slaves_id.insert(slave_id);
    }

    if(m_thread_safe)
        update_slaves_id_lock.unlock();

}
